package fr.utbm.set.safeplatoon.common.merging.data;

import java.io.IOException;
import java.lang.ref.WeakReference;
import java.util.ArrayList;

import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;

import org.arakhne.tinyMAS.core.Kernel;
import org.jdom2.JDOMException;

import fr.utbm.set.safeplatoon.common.merging.Simulation;
import fr.utbm.set.safeplatoon.common.merging.gui.GUI;
import fr.utbm.set.safeplatoon.common.merging.sma.environment.AnimatBody;
import fr.utbm.set.safeplatoon.common.merging.sma.environment.WorldModel;
import fr.utbm.set.safeplatoon.common.merging.util.GeometryUtil;

 


public class Evaluation implements Runnable{

 
	private ArrayList<Point2d> agentsListF = new ArrayList<Point2d>();
	private float maxSpeed = 30;
	private float maxAngle=30;
	
	private Vector2d currentOrder= new Vector2d();

	private GUI gui;
	private WeakReference<Kernel<?, ?, ?, ?>> kernel;
 

	 
	public Evaluation( final Simulation kernel, GUI gui2){
		 
	 
		this.gui=gui2;
		this.kernel = new WeakReference<Kernel<?, ?, ?, ?>>(kernel);
 

	}

	 
 


	public ArrayList<Point2d> getAgentsList() {
		return agentsListF;
	}


	public void setAgentsList(ArrayList<Point2d> agentsList) {
		this.agentsListF = agentsList;
	}


	public float getMaxSpeed() {
		return maxSpeed;
	}


	public void setMaxSpeed(float maxSpeed) {
		this.maxSpeed = maxSpeed;
	}


	public float getMaxAngle() {
		return maxAngle;
	}


	public void setMaxAngle(float maxAngle) {
		this.maxAngle = maxAngle;
	}


	private void eval() throws Exception{

		Vector2d direction = new Vector2d(0,0);
		Vector2d direcrtionMean = new Vector2d(0,0);


		try{


			for (AnimatBody pos :  getEnvironment().getAllAgentBodies()) {
				
				////system.out.println(pos.getLocation());
				if(Double.isNaN(pos.getLocation().x ) || Double.isNaN(pos.getLocation().y ) )
					continue;
				direction.add(new Point2d(pos.getLocation()));
				direcrtionMean.add(new Vector2d(pos.getLinearMove()));
			}

			direction.x/= getEnvironment().getAllAgentBodies().size()*-1;
			direction.y/= getEnvironment().getAllAgentBodies().size();


			//direcrtionMean.x/= getEnvironment().getAllAgentBodies().size()*-1;
			//direcrtionMean.y/= getEnvironment().getAllAgentBodies().size();

 


			this.gui.setTarget(direction);

			//direction.add(direcrtionMean);
			if(direction.length()>this.maxSpeed)
			{
				direction.normalize();
				direction.scale(this.maxSpeed);
			}


		}catch (Exception e) {
			//system.err.println("Creation environement... ");
		}
		
	
		if(direction.x==0 && direction.y==0)
			return;
		
		float speed =  (float) (direction.length()); //FIXME

		float angle =  (float) ( GeometryUtil.signedAngle( direction,new Vector2d(0,1)) );

	
		if(Float.isNaN(angle) || Float.isNaN(speed)){	 
			//system.err.println("NaN");
			return;
		}


	 
		this.gui.setAngle("angle :"+(int) Math.toDegrees(angle));
		this.gui.setSpeed("speed :"+(int)speed);
		
		this.currentOrder=new Vector2d(Math.toDegrees(angle),speed*100);
	}



	public void run()  {
		// TODO Auto-generated method stub
		while(true){

			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}

			
			try {
				this.eval();
			} catch (Exception e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}

		}
	}


	protected WorldModel getEnvironment() {
		return (WorldModel) getKernel().getEnvironment();
	}

	protected Kernel<?, ?, ?, ?> getKernel() {
		return this.kernel.get();
	}

	public void setGui(GUI g) {
		// TODO Auto-generated method stub
		this.gui=g;
	}

	public Vector2d getCurrentOrder() {
		return currentOrder;
	}

	public void setCurrentOrder(Vector2d currentOrder) {
		this.currentOrder = currentOrder;
	}


}
